
#include <algorithm>

#include "errlog.h"


#include <cmath>


#include <iostream>

#include "cxx_misc.h"

#include "LocalSurfaceCalculator.h"

Vector3D PointSet::MeanPoint() const
{
    Vector3D pt(0.0, 0.0, 0.0);
    if(this->size() > 0)
    {
        for(size_t i = 0; i < this->size(); ++i)
            pt += this->at(i);
        pt /= this->size();
    }
    return pt;
}

Matrix3D PointSet::ScatterMatrix(const Vector3D &pt) const
{
    Matrix3D M, foo;
    // Mat = 0.0;  // The default initialization is 0.0
    if(this->size() > 0)
    {
        for(size_t i = 0; i < this->size(); ++i)
        {
            Vector3D wp = this->at(i) - pt;
            //            double dist = wp.length();
            //            if(dist > 1e-12)
            //                wp /= dist;
            M += foo.outer_product(wp, wp);
        }
    }
    return M;
}

bool operator>(const Vector3D &l, const Vector3D &r)
{
    return length2(l.to_array()) > length2(r.to_array());
}

static Vector3D eigen_vector(const Matrix3D &M, double eig_val)
{
    Vector3D r0 = M.row(0) - Vector3D(eig_val, 0.0, 0.0);
    Vector3D r1 = M.row(1) - Vector3D(0.0, eig_val, 0.0);
    Vector3D r2 = M.row(2) - Vector3D(0.0, 0.0, eig_val);
    sort3(r0, r1, r2);

    // check
    Matrix3D A(r0, r1, r2);
    if(fabs(det(A)) > 1e-8)
        error_msg("%s: not an eigenvalue.\n", __func__);
    Vector3D ev = r1 * r2; // cross product
    ev.normalize();
    if(fabs(ev % r0) > 1e-8)
    {
        std::cerr << __func__ << ": not an eigenvalue.\n";
    }
    return ev;
}

static void _eig3(const Matrix3D &M, double *eig)
{
    double coefs[4] = {1, 0, 0, 0};
    double &a = coefs[1], &b = coefs[2], &c = coefs[3];
    a = -M[0][0] - M[1][1] - M[2][2]; // trace of M
    b = M.sub_det(0, 0) + M.sub_det(1, 1) + M.sub_det(2, 2);
    c = -det(M);
#define EVAL_P(X)  ((((X)+a)*(X)+b)*(X)+c)
#define EVAL_P_PRIME(X)  ((3.0*(X)+2.0*a)*(X)+b)
    //  Solving x^3+a*x^2+b*x+c = 0

    //*************************************************************************
    double disc = a * a * (b * b - 4 * a * c)
            - 4 * b * b * b + 18 * a * b * c - 27 * c * c;
    if(disc < 0)
        error_msg("%s: negative discriminant.\n", __func__);
    // if discriminant is negative there is a pair of complex conjugate roots
    // otherwise all roots are real
    //*************************************************************************

    // try to guess one root by Newton's method
    eig[0] = c; //if det(M)==0 then eig[0]=0 is an eigenvalue
    //if not, then eig[0]=det(M) is as good initial guess as any
    int iters = 0, max_iters = 1000; // should converge in few iterations, or not at all
    do
    {
        double p = EVAL_P(eig[0]);
        double delta = -p / EVAL_P_PRIME(eig[0]);
        if(!std::isfinite(delta))
            error_msg("%s: delta is not finite.\n", __func__);
        if(std::max(std::fabs(delta), std::fabs(p)) < 1e-12)
            break;
        eig[0] += delta; // new estimate
        ++iters;
    } while(iters < max_iters);
    if(!(iters < max_iters))
        error_msg("%s: max_iters exceeded.\n", __func__);
    //  factor and use quadratic formula for x^2+(a+eig[0])x+b+a*eig[0]+eig[0]^2
    {
        double p = a + eig[0];
        double q = b + eig[0] * p;
        eig[1] = 0.5 * (-p + sqrt(p * p - 4 * q));
        eig[2] = 0.5 * (-p - sqrt(p * p - 4 * q));
    }
#undef EVAL_P
#undef EVAL_P_PRIME
}

void PointSet::ComputeLocalBasisAtPoint(const Vector3D &pt, Vector3D &t, Vector3D &s, Vector3D &n) const
{
    Matrix3D M = ScatterMatrix(pt);
    double eig[3];
    // find eigenvalues
    /*Matrix3D A = M, Q, R;
    while(fabs(A[1][0]) + fabs(A[2][0]) + fabs(A[2][1]) > 1e-8)
    {
        Mesquite::QR(Q, R, A);
        A = R * Q;
    }
    eig[0] = A[0][0]; eig[1] = A[1][1]; eig[2] = A[2][2]};
     */
    _eig3(M, eig);
    sort3(eig);

    t = eigen_vector(M, eig[2]); //largest eigenvalue
    n = eigen_vector(M, eig[0]); //
    s = n * t;
}

bool PointSet::FitSphereAtPoint(const Vector3D &pt,
        Vector3D &centre, double &radius) const
{
    size_t n = this->size();
    std::vector<Vector3D> M;
    std::vector<double> Q;
    M.resize(n);
    Q.resize(n);
    for(size_t ind = 0; ind < n; ind++)
    {
        M[ind] = this->at(ind) - pt;
        Q[ind] = 0.5 * (M[ind] % (this->at(ind) + pt)); // % is inner product of two Vector3Ds
    }
    Matrix3D A(0.0);
    Vector3D R(0.0, 0.0, 0.0), N;
    for(size_t ind = 0; ind < n; ind++)
    {
        R += Q[ind] * M[ind];
        for(int i = 0; i < 3; i++)
            for(int j = 0; j < 3; j++)
                A[i][j] += M[ind][i] * M[ind][j];
    }

    Matrix3D invA;
    inv(invA, A);
    centre = invA*R;
    radius = dist_l2(pt.to_array(), centre.to_array());
    if(det(A) > 1e-12)
        return true;
    else // points are coplanar
        return false;
}

void PointSet::FitPlane(Vector3D& point, Vector3D& normal) const
{
    point = MeanPoint();
    Vector3D t, s;
    ComputeLocalBasisAtPoint(point, t, s, normal);
}

void PointSet::FitPlaneAtPoint(const Vector3D& point, Vector3D& normal) const
{
    Vector3D t, s;
    ComputeLocalBasisAtPoint(point, t, s, normal);
}

//*****************************************************************************

Vector3D LocalBall::getNormal() const // at FiducialPoint
{
    Vector3D N = FiducialPoint - BallCentre;
    N.normalize();
    return (NormalSign < 0) ? -N : N;
}

Vector3D LocalBall::getNormal(const Vector3D &pt) const // at projection of pt
{
    Vector3D N = pt - BallCentre;
    N.normalize();
    if(std::isfinite(N.x()))
        return (NormalSign < 0) ? -N : N;
    else
        return getNormal();
}

Vector3D LocalBall::getProjection(const Vector3D &pt) const
{

    if(Kappa > 0)
    {
        // line through BallCentre and pt crosses sphere at two points
        Vector3D p1, p2, vec = getNormal(pt);
        p1 = BallCentre + vec / Kappa;
        p2 = BallCentre - vec / Kappa;
        // return the one closer to FiducialPoint
        return ( dist_l2_2(p1.to_array(), FiducialPoint.to_array()) <
                dist_l2_2(p2.to_array(), FiducialPoint.to_array())
                ) ? p1 : p2;
    }
    else
    {
        Vector3D N = getNormal();
        double alpha = (pt - FiducialPoint) % N;
        return pt - alpha*N;

        // plane projection reminder: Gram-Schmidt
        // plane with point N and normal n.
        // projection of point P => C
        // the formula is: C = P - alpha n
        // where alpha = <NP, n> / <n,n>, where vector NP = P - N
    }
}

void LocalBall::computeLocalSurface()
{
    if(AdjacentPoints.FitSphereAtPoint(FiducialPoint, BallCentre, Kappa))
        Kappa = 1.0 / Kappa;
    else
    {
        Vector3D N;
        AdjacentPoints.FitPlaneAtPoint(FiducialPoint, N);
        BallCentre = FiducialPoint - N;
        Kappa = 0.0;
    }
}


